#include <hFramework.h>
#include <stdio.h>
#include "Lego_Light.h"
#include <hMotor.h>
using namespace hFramework;
using namespace hSensors;
int wybor1( int16_t val,int16_t val1, int16_t val2)
{
if(val>val1)
{
return 1;
}else if(val<val2)
{
return 2;
}else if(val<val1&&val>val2)
{
return 3;
}
}

void hMain()
{
sys.setLogDev(&Serial);

	Lego_Light sensor(hSens5);
	int16_t val1,val2,val3;
	LED1.toggle();
	val1=sensor.readRaw();
	printf("val podloża  %5d\r\n", val1);
	val2=val1+75;
	val3=val1-75;
    for(;;)
    {
    int16_t val;
	val=sensor.readRaw();
	printf("val %5d\r\n", val);
	printf("%d\r\n",hMot1.getEncoderCnt());
	printf("%d\r\n",hMot2.getEncoderCnt());
	printf("%d\r\n",hMot3.getEncoderCnt());
	sys.delay_ms(5000);
	int wybor;
   wybor=wybor1(val,val2,val3);
	switch(wybor)
	{
		case 1:
	hMot1.resetEncoderCnt();
	hMot2.resetEncoderCnt();
	hMot3.resetEncoderCnt();
    	hMot3.rotRel(-180,720);
	hMot1.rotRel(-310,320);
	sys.delay_ms(2500);
	hMot1.rotRel(310,320);
	hMot3.rotRel(180,720);
    printf("%d\r\n",hMot1.getEncoderCnt());
    printf("%d\r\n",hMot2.getEncoderCnt());
    printf("%d\r\n",hMot3.getEncoderCnt());
    break;
    case 2:
    	hMot1.resetEncoderCnt();
	hMot2.resetEncoderCnt();
	hMot3.resetEncoderCnt();
	hMot3.rotRel(180,720);
	hMot1.rotRel(-310,320);
	sys.delay_ms(2500);
	hMot1.rotRel(310,320);
    hMot3.rotRel(-180,720);
	printf("%d\r\n",hMot1.getEncoderCnt());
	printf("%d\r\n",hMot2.getEncoderCnt());
	printf("%d\r\n",hMot3.getEncoderCnt());
	break;

case 3:
	hMot1.resetEncoderCnt();
	hMot2.resetEncoderCnt();
	hMot3.resetEncoderCnt();
	hMot1.rotRel(-345,410);
	hMot2.rotRel(-300,400);
	sys.delay_ms(5000);
	hMot1.rotRel(345,410);
	hMot2.rotRel(300,400);
	break;

	}
	}
}